/**
 * anis.darwich
 */

package clubhouse.communication;

import java.io.IOException;

import org.apache.log4j.Logger;
import org.apache.log4j.PropertyConfigurator;

import clubhouse.communication.SerialComm;
import clubhouse.motioncommand.MotionCommand;
import clubhouse.motioncommand.NodeControl;

public class RobotCommunication extends SerialComm implements Runnable{
	
	private final static Logger log = Logger.getLogger(RobotCommunication.class);

	private final int delay = 0;
	private String COM = "COM6";
	
	//private String oldPacket = "";
	private String packet = null;
	
	ReceptionFifo rx;
	TransmissionFifo tx;
	
	public RobotCommunication(ReceptionFifo reception, TransmissionFifo transmission) throws Exception {
		super();
		this.openSerialPort(this.COM, 9600);
		this.rx = reception;
		this.tx = transmission;
		PropertyConfigurator.configure("log4j.properties");
		//this.run();
	}
	

	
	public void sendMotionCommand () {
		MotionCommand tosend = tx.getMotionCom();
		if (tosend != null) {
			try {
				this.sendCommand(tosend.toSend());
			} catch (IOException e) {
				log.debug("RobotCommunication: could not send motion command");
				e.printStackTrace();
			}
		}
	}
	
	public void resetReveivedMessage() {
		this.resetLastResponse();
	}
	
	public static void main(String[] args) throws Exception {
		//PropertyConfigurator.configure("log4j.properties");
		//new RobotCommunication();
	}
	
	protected Object getLock() {
		return this;
	}
	
	public void run() {
		log.debug("RobotCommunication: thread [" + Thread.currentThread().getName() + "]");
		try {
			while (true) {
				synchronized(this.getLock()) {
					
					//Send motion command if available
					this.sendMotionCommand ();
					
					//Check if packet new
					//log.debug(">>> Received " + this.getLastResponse());
					//if (this.getLastResponse() != null) {
					//	if ((this.oldPacket).compareTo(this.getLastResponse()) == 0) {
					//		this.packet = null;
					//	} else {
					//		this.packet = this.getLastResponse();
					//		this.oldPacket = this.getLastResponse();
					//		log.debug("Received: "+ this.packet +"\n");
					//	}
					//}
					
					this.packet = this.getLastResponse();
					this.resetReveivedMessage();
					//Parse and store packet
					if (this.packet != null) {
						switch (Integer.parseInt(packet.substring(2, 4), 16)) {
						case 1:
							NodeControl nodepacket = NodeControl.parse(this.packet);
							//log.debug("Received node control: "+ nodepacket.toHexPrint() +"\n");
							this.rx.rxNodeMeasurement(nodepacket);
							break;
						case 17:
							MotionCommand motionAck = MotionCommand.parse(this.packet);
							//log.debug("Received motion ack: "+ motionAck.toHexPrint() +"\n");
							this.rx.rxMotionAck(motionAck);
							break;
						default:
							log.debug("Received unknown message type\n");
							break;
						}
					}
				}
				//log.debug("RobotCommunication: thread [" + Thread.currentThread().getName() + "] going to sleep "+ delay +" ms\n");
				Thread.sleep(delay);
			}
		} catch (Throwable th) {
			th.printStackTrace();
		}
	}
}
